#include "TMC2209.h"

/*

EN          VM
MS1         GND
MS2         M1B
PDN         M1A
USART       M2A 
CLK         M2B
STEP        VIO
DIR         GND

*/

//4byte数据
void TMC2209_WriteData(uint8_t reg_addr,uint8_t* datagram)
{
    uint8_t write_data[8]={0x05,0x00,0x00,0x00,0x00,0x00,0x00,0x00};

    write_data[2]=0x80|reg_addr;
    for(uint8_t i=0;i<4;i++)
    {
        write_data[3+i]=*(datagram++);
    }
    write_data[7]=TMC2209_calculate_crc(write_data,8);

    for(uint8_t i=0;i<8;i++) 
    {
        USART_SendData(USART2,write_data[i]);
    }
    while (USART_GetFlagStatus(USART2,USART_FLAG_TC)==RESET);

    delay_ms(1);
}

ErrorStatus TMC2209_ReadData(uint8_t reg_addr,uint8_t* receive_data)
{
    uint8_t datagram[4]={0x05,0x00,0x00,0x00};
    uint32_t timeout=0xfffffff;
    uint32_t time_count=0;
    uint8_t data_count=0;

    datagram[2]=0x7F&reg_addr;
    datagram[3]=TMC2209_calculate_crc(datagram,4);

    for(uint8_t i=0;i<4;i++) 
    {
        USART_SendData(USART2,datagram[i]);
    }

    while (USART_GetFlagStatus(USART2,USART_FLAG_TC)==RESET);
    
    TMC2209UartTxOff();
    
    while (data_count<9)
    {
        if(USART_GetFlagStatus(USART2,USART_FLAG_RXNE)==SET){
            *(receive_data++)=USART_ReceiveData(USART2);
            data_count++;
            time_count=0;
        }   

        if(time_count++>timeout)
        {
            return ERROR;
        }
    }

    TMC2209UartTxOn();

    delay_ms(1);

    return SUCCESS;
}


uint8_t TMC2209_calculate_crc(uint8_t* datagram,uint8_t datagramLength)
{
    int i,j;
    uint8_t crc=0;
    uint8_t currentByte;

    for(i=0;i<(datagramLength-1);i++){
        currentByte=datagram[i];
        for(j=0;j<8;j++){
            if((crc>>7)^(currentByte&0x01))
            {
                crc=(crc<<1)^0x07;
            }
            else
            {
                crc=(crc<<1);
            }
            currentByte=currentByte>>1;
        }
    }

    return crc;
}
